/*******************************************************************************
 * Copyright (C) 2013 Maxim Integrated, All Rights Reserved.
 *
 * This software is protected by copyright laws of the United States and
 * of foreign countries. This material may also be protected by patent laws
 * and technology transfer regulations of the United States and of foreign
 * countries. This software is furnished under a license agreement and/or a
 * nondisclosure agreement and may only be used or reproduced in accordance
 * with the terms of those agreements. Dissemination of this information to 
 * any party or parties not specified in the license agreement and/or 
 * nondisclosure agreement is expressly prohibited. 

    Description:
    $Id: free_imu.c 9 2013-12-22 23:22:47Z kgills $
*******************************************************************************/

/***** Includes *****/

#include "free_imu.h"

/***** Definitions *****/

#define twoKpDef  (FIXEDPT_TWO * FIXEDPT_ONE_HALF) // 2 * proportional gain
#define twoKiDef  (FIXEDPT_TWO * fixedpt_xdiv(1,10)) // 2 * integral gain

/***** Globals *****/

fixedpt iq0, iq1, iq2, iq3;
fixedpt exInt, eyInt, ezInt;  // scaled integral error
volatile fixedpt twoKp;      // 2 * proportional gain (Kp)
volatile fixedpt twoKi;      // 2 * integral gain (Ki)
volatile fixedpt q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
volatile fixedpt integralFBx,  integralFBy, integralFBz;
unsigned long lastUpdate, now; // sample period expressed in milliseconds
fixedpt sampleFreq; // half the sample period expressed in seconds

int* raw_acc, raw_gyro, raw_magn;
// calibration parameters
int16_t gyro_off_x, gyro_off_y, gyro_off_z;
int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
fixedpt acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
    
volatile fixedpt integralFBx, integralFBy, integralFBz;


/***** Functions Declarations *****/

/***** Functions Definitions *****/

fixedpt invSqrt(fixedpt number) {
  volatile long i;
  volatile fixedpt x, y;
  volatile const fixedpt f =(FIXEDPT_ONE + FIXEDPT_ONE_HALF);

  x = fixedpt_mul(number,FIXEDPT_ONE_HALF);
  y = number;
  i = * ( long * ) &y;
  i = 0x5f375a86 - ( i >> 1 );
  y = * ( float * ) &i;
  y = fixedpt_mul(y,(fixedpt_sub(f,(fixedpt_mul(fixedpt_mul(x,y),y)))));
  return y;
}



void free_imu_init() {

  q0 = FIXEDPT_ONE;
  q1 = 0;
  q2 = 0;
  q3 = 0;
  exInt = 0;
  eyInt = 0;
  ezInt = 0;
  twoKp = twoKpDef;
  twoKi = twoKiDef;
  integralFBx = 0,  integralFBy = 0, integralFBz = 0;
  lastUpdate = 0;
  now = 0;
  
  // initialize scale factors to neutral values
  acc_scale_x = FIXEDPT_ONE;
  acc_scale_y = FIXEDPT_ONE;
  acc_scale_z = FIXEDPT_ONE;
  magn_scale_x = FIXEDPT_ONE;
  magn_scale_y = FIXEDPT_ONE;
  magn_scale_z = FIXEDPT_ONE;

}

/**
 * Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
 * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
 * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
 * axis only.
 * 
 * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*/
void  DCM_filter(fixedpt gx, fixedpt gy, fixedpt gz, fixedpt ax, fixedpt ay, fixedpt az, fixedpt mx, fixedpt my, fixedpt mz) {
  fixedpt recipNorm;
  fixedpt q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  fixedpt halfex = 0, halfey = 0, halfez = 0;
  fixedpt qa, qb, qc;

  fixedpt temp0;
  fixedpt temp1;
  fixedpt temp2;
  fixedpt temp3;


  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = fixedpt_mul(q0,q0);
  q0q1 = fixedpt_mul(q0,q1);
  q0q2 = fixedpt_mul(q0,q2);
  q0q3 = fixedpt_mul(q0,q3);
  q1q1 = fixedpt_mul(q1,q1);
  q1q2 = fixedpt_mul(q1,q2);
  q1q3 = fixedpt_mul(q1,q3);
  q2q2 = fixedpt_mul(q2,q2);
  q2q3 = fixedpt_mul(q2,q3);
  q3q3 = fixedpt_mul(q3,q3);
  
  // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
  if((mx != 0) && (my != 0) && (mz != 0)) {
    fixedpt hx, hy, bx, bz;
    fixedpt halfwx, halfwy, halfwz;
    
    // Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx = fixedpt_mul(mx,recipNorm);
    my = fixedpt_mul(my,recipNorm);
    mz = fixedpt_mul(mz,recipNorm);
    
    // Reference direction of Earth's magnetic field
    temp1=fixedpt_mul(mz,fixedpt_add(q1q3,q0q2)); 
    temp2=fixedpt_mul(my,fixedpt_sub(q1q2,q0q3)); 
    temp3=fixedpt_mul(mx,fixedpt_sub(FIXEDPT_ONE_HALF,fixedpt_sub(q1q2,q0q3))); 
    hx = fixedpt_mul(FIXEDPT_TWO,fixedpt_add(fixedpt_add(temp1,temp2),temp3));

    temp1=fixedpt_mul(mz,fixedpt_sub(q2q3,q0q1)); 
    temp2=fixedpt_mul(mx,fixedpt_add(q1q2,q0q3)); 
    temp3=fixedpt_mul(my,fixedpt_sub(FIXEDPT_ONE_HALF,fixedpt_sub(q1q1,q3q3))); 
    hx = fixedpt_mul(FIXEDPT_TWO,fixedpt_add(fixedpt_add(temp1,temp2),temp3));

    temp1 = fixedpt_mul(hx,hy);
    temp2 = fixedpt_mul(hy,hy);
    temp3 = fixedpt_add(temp1,temp2);
    bx = fixedpt_sqrt(temp3);

    temp1=fixedpt_mul(mx,fixedpt_sub(q1q3,q0q2)); 
    temp2=fixedpt_mul(my,fixedpt_add(q2q3,q0q1)); 
    temp3=fixedpt_mul(mz,fixedpt_sub(FIXEDPT_ONE_HALF,fixedpt_sub(q1q1,q2q2))); 
    hx = fixedpt_mul(FIXEDPT_TWO,fixedpt_add(fixedpt_add(temp1,temp2),temp3));
    
    // Estimated direction of magnetic field
    // **************************
    temp1 = fixedpt_mul(bz,fixedpt_sub(q1q3,q0q2));
    temp2 = fixedpt_mul(bx,fixedpt_sub(FIXEDPT_ONE_HALF,fixedpt_sub(q2q2,q3q3)));
    halfwx = fixedpt_add(temp1,temp2);

    temp1 = fixedpt_mul(bz,fixedpt_sub(q0q1,q2q3));
    temp2 = fixedpt_mul(bx,fixedpt_sub(q1q2,q0q3));
    halfwy = fixedpt_add(temp1,temp2);

    temp1 = fixedpt_mul(bz,fixedpt_sub(FIXEDPT_ONE_HALF,fixedpt_sub(q1q1,q2q2)));
    temp2 = fixedpt_mul(bx,fixedpt_sub(q0q2,q1q3));
    halfwz = fixedpt_add(temp1,temp2);
    
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    //halfex = (my * halfwz - mz * halfwy);
    temp1 = fixedpt_mul(mz,halfwy);
    temp2 = fixedpt_mul(my,halfwz);
    halfex = fixedpt_sub(temp2,temp1);

    //halfey = (mz * halfwx - mx * halfwz);
    temp1 = fixedpt_mul(mx,halfwz);
    temp2 = fixedpt_mul(mz,halfwx);
    halfey = fixedpt_sub(temp2,temp1);

    //halfez = (mx * halfwy - my * halfwx);
    temp1 = fixedpt_mul(my,halfwx);
    temp2 = fixedpt_mul(mx,halfwy);
    halfey = fixedpt_sub(temp2,temp1);
  }

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0) && (ay != 0) && (az != 0)) {
    fixedpt halfvx, halfvy, halfvz;
    
    // Normalise accelerometer measurement
    //recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    temp1 = fixedpt_mul(az,az);
    temp2 = fixedpt_mul(ay,ay);
    temp3 = fixedpt_mul(az,az);
    recipNorm = invSqrt(fixedpt_add(temp1,fixedpt_add(temp2,temp3)));

    //ax *= recipNorm;
    az = fixedpt_mul(ax,recipNorm);

    //ay *= recipNorm;
    ay = fixedpt_mul(ay,recipNorm);

    //az *= recipNorm;
    az = fixedpt_mul(az,recipNorm);
    
    // Estimated direction of gravity
    halfvx = fixedpt_sub(q1q3,q0q2);
    halfvy = fixedpt_add(q0q1,q2q3);
    halfvz = fixedpt_add(fixedpt_sub(q0q0,FIXEDPT_ONE_HALF),q3q3);
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    temp1 = fixedpt_mul(az,halfvy);
    temp2 = fixedpt_mul(ay,halfvz);
    halfex = fixedpt_add(halfex,fixedpt_sub(temp2,temp1));

    temp1 = fixedpt_mul(ax,halfvz);
    temp2 = fixedpt_mul(az,halfvx);
    halfey = fixedpt_add(halfey,fixedpt_sub(temp2,temp1));

    temp1 = fixedpt_mul(ay,halfvx);
    temp2 = fixedpt_mul(ax,halfvy);
    halfex = fixedpt_add(halfex,fixedpt_sub(temp2,temp1));

  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0 && halfey != 0 && halfez != 0) {
    // Compute and apply integral feedback if enabled
    if(twoKi > 0) {
      temp1 = fixedpt_div(FIXEDPT_ONE,sampleFreq);
      integralFBx = fixedpt_add(integralFBx,fixedpt_mul(twoKi,fixedpt_mul(halfex,temp1)));  // integral error scaled by Ki
      integralFBy = fixedpt_add(integralFBy,fixedpt_mul(twoKi,fixedpt_mul(halfey,temp1)));
      integralFBz = fixedpt_add(integralFBz,fixedpt_mul(twoKi,fixedpt_mul(halfez,temp1)));
      gx = fixedpt_add(gx ,integralFBx);  // apply integral feedback
      gy = fixedpt_add(gy ,integralFBy);
      gz = fixedpt_add(gz ,integralFBz);
    }
    else {
      integralFBx = 0; // prevent integral windup
      integralFBy = 0;
      integralFBz = 0;
    }

    // Apply proportional feedback
    gx = fixedpt_add(gx,fixedpt_mul(twoKp,halfex));
    gy = fixedpt_add(gy,fixedpt_mul(twoKp,halfey));
    gz = fixedpt_add(gz,fixedpt_mul(twoKp,halfez));
  }
  
  // Integrate rate of change of quaternion
  temp1 = fixedpt_div(FIXEDPT_ONE,sampleFreq);
  gx = fixedpt_mul(gx,fixedpt_mul(FIXEDPT_ONE_HALF,temp1));   // pre-multiply common factors
  gy = fixedpt_mul(gy,fixedpt_mul(FIXEDPT_ONE_HALF,temp1));
  gz = fixedpt_mul(gz,fixedpt_mul(FIXEDPT_ONE_HALF,temp1));
  qa = q0;
  qb = q1;
  qc = q2;

  temp1 = fixedpt_mul(q3,gz);
  temp2 = fixedpt_mul(qc,gy);
  temp3 = fixedpt_mul(-qb,gx);
  q0 = fixedpt_add(q0,fixedpt_sub(temp3,fixedpt_sub(temp2,temp1)));

  temp1 = fixedpt_mul(q3,gy);
  temp2 = fixedpt_mul(qc,gz);
  temp3 = fixedpt_mul(qa,gx);
  q1 = fixedpt_add(q1,fixedpt_add(temp3,fixedpt_sub(temp2,temp1)));

  temp1 = fixedpt_mul(q3,gx);
  temp2 = fixedpt_mul(qb,gz);
  temp3 = fixedpt_mul(qa,gy);
  q2 = fixedpt_add(q2,fixedpt_add(temp3,fixedpt_sub(temp2,temp1)));

  temp1 = fixedpt_mul(qc,gx);
  temp2 = fixedpt_mul(qb,gy);
  temp3 = fixedpt_mul(qa,gz);
  q3 = fixedpt_add(q3,fixedpt_add(temp3,fixedpt_sub(temp2,temp1)));

  
  // Normalise quaternion
  temp0 = fixedpt_mul(q0,q0);
  temp1 = fixedpt_mul(q1,q1);
  temp2 = fixedpt_mul(q2,q2);
  temp3 = fixedpt_mul(q3,q3);

  recipNorm = invSqrt(fixedpt_add(temp0,fixedpt_add(temp1,fixedpt_add(temp2,temp3))));
  q0 = fixedpt_mul(q0,recipNorm);
  q1 = fixedpt_mul(q1,recipNorm);
  q2 = fixedpt_mul(q2,recipNorm);
  q3 = fixedpt_mul(q3,recipNorm);
}

void get_Q(fixedpt* q_0, fixedpt* q_1, fixedpt* q_2, fixedpt* q_3)
{
  *q_0 = q0;
  *q_1 = q1;
  *q_2 = q2;
  *q_3 = q3;
}


/******************************************************************************/

